#include "invertedPendulum.h"
#include <iostream>

using namespace std;

InvertedPendulum::InvertedPendulum() {
	quadric = gluNewQuadric();
}

void InvertedPendulum::startSym(int width, int height, float camAngle) {
	SDL_Init(SDL_INIT_EVERYTHING);
	SDL_SetVideoMode(width, height, 0, SDL_OPENGL);
	glClearColor(0, 0, 0, 1);
	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	gluPerspective(camAngle, double(width) / double(height), 1, 500);
	glMatrixMode(GL_MODELVIEW);
	glEnable(GL_DEPTH_TEST);
	camera.setLocation(Vector3D(0.75f,13.8f,57.77f));
	Uint32 start;
	SDL_Event event;
	btTransform t;
	bool running = true;
	while (running) {
		start = SDL_GetTicks();
		while (SDL_PollEvent(&event)) {
			switch (event.type) {
			case SDL_QUIT:
				running = false;
				break;
			case SDL_KEYDOWN:
				switch (event.key.keysym.sym) {
				case SDLK_ESCAPE:
					running = false;
					break;
				case SDLK_y:
					camera.mouseIn = false;
					break;
				case SDLK_SPACE:
					break;
				}
				break;
			case SDL_MOUSEBUTTONDOWN:
				camera.mouseIn = true;
				break;
			}
		}
		world->stepSimulation(1.f / 60.f);
		updateDisplay();
		/*cout << "Camera Position\nX:" << camera.getLocation().x << " Y:"
				<< camera.getLocation().y << " Z:" << camera.getLocation().z
				<< endl;*/
		if (1000.0 / 60 > SDL_GetTicks() - start)
			SDL_Delay(1000.0 / 60 - (SDL_GetTicks() - start));
	}
}

void InvertedPendulum::initPhysics() {
	collisionConfig = new btDefaultCollisionConfiguration();
	dispatcher = new btCollisionDispatcher(collisionConfig);
	broadphase = new btDbvtBroadphase();
	solver = new btSequentialImpulseConstraintSolver();
	world = new btDiscreteDynamicsWorld(dispatcher, broadphase, solver,
			collisionConfig);
	world->setGravity(btVector3(0, -9.8f, 0));
	btRigidBody *plane = addPlane(btVector3(0, -2, 0));
	btRigidBody *base = addBox(btVector3(0, -1, 0), 44.0f, 2.0f, 2.0f, 0.f);
	btRigidBody *car = addBox(btVector3(0, 0, 0), 2.0f, 2.0f, 2.0f, 1.0f);
	btRigidBody *cylinder = addCylinder(btVector3(0, 0, 7), 0.4f, 3.0f, 1.0f);
}

void InvertedPendulum::termPhysics() {

	for (int i = world->getNumCollisionObjects() - 1; i >= 0; i--) {
		btCollisionObject* obj = world->getCollisionObjectArray()[i];
		btRigidBody* body = btRigidBody::upcast(obj);
		if (body && body->getMotionState()) {
			delete body->getMotionState();
		}
		world->removeCollisionObject(obj);
		delete obj;
	}
	for (int j = 0; j < collisionShapes.size(); j++) {
		btCollisionShape* shape = collisionShapes[j];
		delete shape;
	}

	delete world;
	delete solver;
	delete broadphase;
	delete dispatcher;
	delete collisionConfig;
}

InvertedPendulum::~InvertedPendulum() {
	termPhysics();
	SDL_Quit();
	gluDeleteQuadric(quadric);
}

btRigidBody* InvertedPendulum::addBox(btVector3 origin, float width,
		float height, float depth, float mass) {
	btTransform t;
	t.setIdentity();
	t.setOrigin(origin);
	btBoxShape* box = new btBoxShape(
			btVector3(width / 2.0, height / 2.0, depth / 2.0));
	collisionShapes.push_back(box);
	btVector3 inertia(0, 0, 0);
	if (mass != 0.0)
		box->calculateLocalInertia(mass, inertia);

	btMotionState* motion = new btDefaultMotionState(t);
	btRigidBody::btRigidBodyConstructionInfo info(mass, motion, box, inertia);
	btRigidBody* body = new btRigidBody(info);
	world->addRigidBody(body);
	return body;
}

btRigidBody* InvertedPendulum::addCylinder(btVector3 origin, float diameter,
		float height, float mass) {
	btTransform t;
	t.setIdentity();
	t.setOrigin(origin);
	btCylinderShape* cylinder = new btCylinderShape(
			btVector3(diameter / 2.0, height / 2.0, diameter / 2.0));
	collisionShapes.push_back(cylinder);
	btVector3 inertia(0, 0, 0);
	if (mass != 0.0)
		cylinder->calculateLocalInertia(mass, inertia);

	btMotionState* motion = new btDefaultMotionState(t);
	btRigidBody::btRigidBodyConstructionInfo info(mass, motion, cylinder,
			inertia);
	btRigidBody* body = new btRigidBody(info);
	world->addRigidBody(body);
	return body;
}

btRigidBody* InvertedPendulum::addPlane(btVector3 origin) {
	btTransform t;
	t.setIdentity();
	t.setOrigin(origin);
	btStaticPlaneShape* plane = new btStaticPlaneShape(btVector3(0, 1, 0), 0);
	collisionShapes.push_back(plane);
	btMotionState* motion = new btDefaultMotionState(t);
	btRigidBody::btRigidBodyConstructionInfo info(0.0, motion, plane);
	btRigidBody* body = new btRigidBody(info);
	world->addRigidBody(body);
	return body;
}

void InvertedPendulum::renderObjects() {
	for (int i = world->getNumCollisionObjects() - 1; i >= 0; i--) {
		btCollisionObject* obj = world->getCollisionObjectArray()[i];
		btRigidBody* body = btRigidBody::upcast(obj);
		switch (body->getCollisionShape()->getShapeType()) {
		case STATIC_PLANE_PROXYTYPE:
			renderPlane(body);
			break;
		case BOX_SHAPE_PROXYTYPE:
			renderBox(body);
			break;
		case CYLINDER_SHAPE_PROXYTYPE:
			renderCylinder(body);
			break;
		default:
			break;
		}
	}
}

void InvertedPendulum::renderBox(btRigidBody *body) {
	if (body->getCollisionShape()->getShapeType() != BOX_SHAPE_PROXYTYPE)
		return;
	glColor3f(0.8, 0.8, 0.8);
	btVector3 extent =
			((btBoxShape*) body->getCollisionShape())->getHalfExtentsWithoutMargin();
	btTransform t;
	body->getMotionState()->getWorldTransform(t);
	float mat[16];
	t.getOpenGLMatrix(mat);
	glPushMatrix();
	glMultMatrixf(mat);
	glBegin(GL_QUADS);
	glVertex3f(-extent.x(), extent.y(), -extent.z());
	glVertex3f(-extent.x(), -extent.y(), -extent.z());
	glVertex3f(-extent.x(), -extent.y(), extent.z());
	glVertex3f(-extent.x(), extent.y(), extent.z());
	glEnd();
	glBegin(GL_QUADS);
	glVertex3f(extent.x(), extent.y(), -extent.z());
	glVertex3f(extent.x(), -extent.y(), -extent.z());
	glVertex3f(extent.x(), -extent.y(), extent.z());
	glVertex3f(extent.x(), extent.y(), extent.z());
	glEnd();
	glBegin(GL_QUADS);
	glVertex3f(-extent.x(), extent.y(), extent.z());
	glVertex3f(-extent.x(), -extent.y(), extent.z());
	glVertex3f(extent.x(), -extent.y(), extent.z());
	glVertex3f(extent.x(), extent.y(), extent.z());
	glEnd();
	glBegin(GL_QUADS);
	glVertex3f(-extent.x(), extent.y(), -extent.z());
	glVertex3f(-extent.x(), -extent.y(), -extent.z());
	glVertex3f(extent.x(), -extent.y(), -extent.z());
	glVertex3f(extent.x(), extent.y(), -extent.z());
	glEnd();
	glBegin(GL_QUADS);
	glVertex3f(-extent.x(), extent.y(), -extent.z());
	glVertex3f(-extent.x(), extent.y(), extent.z());
	glVertex3f(extent.x(), extent.y(), extent.z());
	glVertex3f(extent.x(), extent.y(), -extent.z());
	glEnd();
	glBegin(GL_QUADS);
	glVertex3f(-extent.x(), -extent.y(), -extent.z());
	glVertex3f(-extent.x(), -extent.y(), extent.z());
	glVertex3f(extent.x(), -extent.y(), extent.z());
	glVertex3f(extent.x(), -extent.y(), -extent.z());
	glEnd();
	glPopMatrix();
}

void InvertedPendulum::renderCylinder(btRigidBody *body) {
	if (body->getCollisionShape()->getShapeType() != CYLINDER_SHAPE_PROXYTYPE)
		return;
	glColor3f(0.8, 0.8, 0.8);
	btVector3 extent =
			((btCylinderShape*) body->getCollisionShape())->getHalfExtentsWithoutMargin();
	btTransform t;
	body->getMotionState()->getWorldTransform(t);
	float mat[16];
	t.getOpenGLMatrix(mat);
	glPushMatrix();
	glMultMatrixf(mat);
	//glTranslatef(0, extent.y(), 0);
	//glRotatef(90, 1, 0, 0);
	gluCylinder(quadric, extent.x(), extent.x(), extent.y() * 2.0, 20, 20);
	glPopMatrix();
}

void InvertedPendulum::renderPlane(btRigidBody *body) {
	if (body->getCollisionShape()->getShapeType() != STATIC_PLANE_PROXYTYPE)
		return;
	glColor3f(1, 1, 1);
	btTransform t;
	body->getMotionState()->getWorldTransform(t);
	float mat[16];
	t.getOpenGLMatrix(mat);
	glPushMatrix();
	glMultMatrixf(mat);
	glBegin(GL_QUADS);
	glVertex3f(-1000, 0, 1000);
	glVertex3f(-1000, 0, -1000);
	glVertex3f(1000, 0, -1000);
	glVertex3f(1000, 0, 1000);
	glEnd();
	glPopMatrix();
}

void InvertedPendulum::updateDisplay() {
	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
	glLoadIdentity();
	camera.Control();
	camera.updateCamera();
	renderObjects();
	SDL_GL_SwapBuffers();
}
